/*
** Copyright (C) 2009 Joshua Brent Marsh <joshua@icub3d.com>
**  
** This file is part of icub3d-adt
**
** icub3d-adt is free sofware; you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation; either version 3 of the License, or
** (at your option) any later version.
**  
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
** GNU General Public License for more details.
**  
** You should have received a copy of the GNU General Public License
** along with this program; if not, write to the Free Software
** Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
**  
*/

/* $Id: pqueue.c 42 2009-11-13 18:50:14Z joshua@icub3d.com $ */


#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdlib.h>

#include <assert.h>

#include "queue.h"
#include "btree_funcs.h"
#include "pqueue.h"

/* These are used for the elements of the binary tree. */
typedef
struct pqueue_pair
{
  size_t id;
  Queue * items;
  destroy_func *dfunc;
} PQPair;

typedef
struct phueue_pair
{
  size_t id;
  void * item;
  destroy_func *dfunc;
} PHPair;

PQPair * 
create_pqpair (const size_t id, Queue *items, destroy_func *dfunc)
{
  PQPair *nnode;

  nnode = malloc (sizeof (PQPair));
  if (nnode == NULL)
    return NULL;

  nnode->id = id;
  nnode->dfunc = dfunc;
  nnode->items = items;

  return nnode;
}

PHPair * 
create_phpair (const size_t id, void *item, destroy_func *dfunc)
{
  PHPair *nnode;

  nnode = malloc (sizeof (PHPair));
  if (nnode == NULL)
    return NULL;

  nnode->id = id;
  nnode->item = item;
  nnode->dfunc = dfunc;

  return nnode;
}

/* Comparer for our tree. */
int
comp_pqpair (const void *a, const void *b)
{
  const PQPair *first = a;
  const PQPair *second = b;

  if (first->id < second->id)
    return -1;
  else if (first->id > second->id)
    return 1;
  else
    return 0;
}

int
comp_phpair (const void *a, const void *b)
{
  const PHPair *first = a;
  const PHPair *second = b;

  if (first->id < second->id)
    return -1;
  else if (first->id > second->id)
    return 1;
  else
    return 0;
}

int
comp_rphpair (const void *a, const void *b)
{
  const PHPair *first = a;
  const PHPair *second = b;

  if (first->id > second->id)
    return -1;
  else if (first->id < second->id)
    return 1;
  else
    return 0;
}

/* cleans up our tree. */
void
dest_pqpair (void *a)
{
  PQPair *first = a;

  queue_destroy (first->items, first->dfunc);
  free (first);
}

void
dest_phpair (void *a)
{
  PHPair *first = a;

  if (first->dfunc != NULL)
    first->dfunc (first->item);

  free (first);
}

PQueue * 
pqueue_create (PQueueType type, destroy_func *dfunc)
{
  PQueue *queue;

  queue = malloc (sizeof (PQueue));
  if (queue == NULL)
    return NULL;

  if (type == MIN || type == MAX)
    {
      queue->storage.tree = btree_create (comp_pqpair, RB);
      if (queue->storage.tree == NULL)
	{
	  free (queue);
	  return NULL;
	}
    }
  else
    {
      queue->storage.heap = heap_create (type == HEAP_MIN ? 
					  comp_phpair : comp_rphpair);
      if (queue->storage.heap == NULL)
	{
	  free (queue);
	  return NULL;
	}      
    }

  queue->type = type;
  queue->dfunc = dfunc;
  queue->size = 0;

  return queue;
}

void
pqueue_destroy (PQueue *queue)
{
  assert (queue != NULL);

  if (queue->type == MIN || queue->type == MAX)
    btree_destroy (queue->storage.tree, dest_pqpair);
  else
    heap_destroy (queue->storage.heap, dest_phpair, 1);

  free (queue);
}

void * 
pqueue_enqueue (PQueue *queue, size_t priority, void *item)
{
  if (queue->type == MIN || queue->type == MAX)
    {
      PQPair search;
      PQPair *np, *res;

      np = create_pqpair (priority, NULL, queue->dfunc);
      if (np == NULL)
	return NULL;

      res = btree_insert (queue->storage.tree, np);
      if (res == np)
	{
	  /* Insert succeeded, make queue. */
	  np->items = queue_create ();
	  if (np->items == NULL)
	    {
	      search.id = priority;
	      res = btree_remove (queue->storage.tree, &search);
	      dest_pqpair (np);
	      return NULL;
	    }
	  
	  if (queue_enqueue (np->items, item) == 0)
	    {
	      search.id = priority;
	      res = btree_remove (queue->storage.tree, &search);
	      dest_pqpair (np);
	      return NULL;
	    }
	  else
	    {
	      queue->size++;
	      return item;
	    }

	}
      else if (res == NULL)
	{
	  /* Error. */
	  free (np);
	  return NULL;
	}
      else
	{
	  /* We already have this priority. */
	  free (np);
	}
      
      if (queue_enqueue (res->items, item) == 0)
	return NULL;
      else
	{
	  queue->size++;
	  return item;
	}
    }
  else
    {
      PHPair *np;

      np = create_phpair (priority, item, queue->dfunc);
      if (np == NULL)
	return NULL;

      if (heap_insert (queue->storage.heap, np) == 1)
	{
	  queue->size++;
	  return item;
	}
      else
	return NULL;
    }
}

/* A helper for the find and remove firsts. */
struct btree_node * 
get_node (Btree *tree, PQueueType type)
{
  struct btree_node *cur;

  if (type == MIN)
    cur = find_min (tree);
  else
    cur = find_max (tree);

  if (cur == NULL)
    return NULL;
  else
    return cur;
}

void * 
pqueue_peek (PQueue *queue)
{
  assert (queue != NULL);

  if (queue->type == MIN || queue->type == MAX)
    {
      struct btree_node *n;
      PQPair * i;

      n = get_node (queue->storage.tree, queue->type);
      if (n == NULL)
	return NULL;
      
      i = (PQPair *)n->data;
      
      return queue_peek (i->items);
    }
  else
    {
      PHPair * i = heap_find_min (queue->storage.heap);

      return i->item;
    }
}

void * 
pqueue_dequeue (PQueue *queue)
{
  void * ret;

  assert (queue != NULL);

  if (queue->type == MIN || queue->type == MAX)
    {
      struct btree_node *n;
      PQPair * i;

      n = get_node (queue->storage.tree, queue->type);
      if (n == NULL)
	return NULL;
      
      i = (PQPair *)n->data;
      
      ret = queue_dequeue (i->items);
      
      if (queue_size (i->items) == 0)
	{
	  remove_node (queue->storage.tree, n);
	  dest_pqpair (i);
	}
    }
  else
    {
      PHPair *i = heap_extract_min (queue->storage.heap);
      if (i == NULL)
	return NULL;

      ret = i->item;
      i->dfunc = NULL;
      dest_phpair (i);
    }

  queue->size--;
  
  return ret;
}

